#include "motion_control.h"
#include "stdio.h"
#include "adc.h"

extern motor __motor;

void motor_align(p_motor motor)
{
    HAL_NVIC_DisableIRQ(TIM1_UP_TIM10_IRQn);    //stop pwm NVIC
    HAL_NVIC_DisableIRQ(EXTI1_IRQn);    //stop encoder z NVIC
    motor->status.align = 1;//start align

    motor->id_pid.out = 1.0f;//turn the rotor to aix A
    motor->iq_pid.out = 0.0f;
    motor->encoder.radian = 0.0f;
    anti_park(motor, motor->id_pid.out, motor->iq_pid.out);
    sector_judgment(motor);
    pwm_duration_calc(motor);
    pwm_out(motor);
    HAL_Delay(100);//wait for motor stable
    TIM3->CNT = 0; //reset encoder capture value

    static float align_theta;
    while(motor->status.align)//untile encoder phase z to change this value
    {
        align_theta += 0.00001;
        motor->id_pid.out = 0.001;
        motor->iq_pid.out = 0.8;
        motor->encoder.radian = align_theta;
        anti_park(motor, motor->id_pid.out, motor->iq_pid.out);
        sector_judgment(motor);
        pwm_duration_calc(motor);
        pwm_out(motor);
    }
    
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    if(GPIO_Pin == EN_Z_Pin)
    {
        __motor.status.align = 0;
        __motor.encoder.offset = TIM3->CNT;
    }
}

void current_calTest(p_motor motor)
{
    TIM1->CCR1 = 4200;
    TIM1->CCR2 = 4200;
    TIM1->CCR3 = 4200;
    HAL_Delay(100);

}

void motor_run(p_motor motor)
{
    if(motor->status.enable == MOTOR_DIS)
    {
        HAL_NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
        TIM1->CCR1 = 4200;
        TIM1->CCR2 = 4200;
        TIM1->CCR3 = 4200;
        TIM1->CCR4 = 8300;
        
        
    }
}
/* 
RUN_Pin GPIO_PIN_5  RUN_GPIO_Port GPIOC UP_Pin GPIO_PIN_10 UP_GPIO_Port GPIOB 
DOWN_Pin GPIO_PIN_11 DOWN_GPIO_Port GPIOB DIR_Pin GPIO_PIN_12 DIR_GPIO_Port GPIOB 
*/
void key_scan(void)
{
    static uint8_t gpio_jitFilterCount; //global filter counter for eatch gpio input
    //only one key can be trigged at one time
    if(GPIO_PIN_RESET == HAL_GPIO_ReadPin(RUN_GPIO_Port, RUN_Pin))
    {
        gpio_jitFilterCount++;
        if(gpio_jitFilterCount >= 100) //100ms key input jitter filter time
        {
            gpio_jitFilterCount = 0;
        }
    }
    else if(GPIO_PIN_RESET == HAL_GPIO_ReadPin(UP_GPIO_Port, UP_Pin))
    {
        gpio_jitFilterCount++;
        if(gpio_jitFilterCount >= 100) //100ms key input jitter filter time
        {
            gpio_jitFilterCount = 0;
        }
        
    }
    else if(GPIO_PIN_RESET == HAL_GPIO_ReadPin(DOWN_GPIO_Port, DOWN_Pin))
    {
       gpio_jitFilterCount++;
        if(gpio_jitFilterCount >= 100) //100ms key input jitter filter time
        {
            gpio_jitFilterCount = 0;
        } 
    }
    else if(GPIO_PIN_RESET == HAL_GPIO_ReadPin(DIR_GPIO_Port, DIR_Pin))
    {
       gpio_jitFilterCount++;
        if(gpio_jitFilterCount >= 100) //100ms key input jitter filter time
        {
            gpio_jitFilterCount = 0;
        } 
    }
    else
    {
        gpio_jitFilterCount = 0;
    }
}